//////////////////////////////////////////////////////////////////
//©2011 bildr
//Released under the MIT License - Please reuse change and share
//Using the easy stepper with your arduino
//use rotate and/or rotateDeg to control stepper motor
//speed is any number from .01 -> 1 with 1 being fastest - 
//Slower Speed == Stronger movement


//notes:
// wiring diagram here: http://bildr.org/2011/06/easydriver/#
// arduino mega reference here: http://arduino.cc/en/Main/ArduinoBoardMega2560
// writing/reading with pins: http://www.arduino.cc/en/Reference/SoftwareSerial
//  ->supports Serial Peripheral Protocol (SPI) and Two-wire I2C (TWI)
// hbridge ref: http://www.tigoe.net/pcomp/code/circuits/motors/controlling-dc-motors
// HOW DO I X: http://arduino.cc/en/Tutorial/HomePage
/////////////////////////////////////////////////////////////////

//Main code written by Team 29 for Carnegie Mellon University class 18-549 project MagChess

// Pins used for sending data to motor driver
#define STEP_PIN_A 2 //output pin for direction of stepper motor
#define DIR_PIN_A 4 //output pin used to tell motor to move a step
#define STEP_PIN_B 5
#define DIR_PIN_B 6

//Input pins to control motor driver "A"
#define INDIR_PIN_A 40 //direction
#define STEP_A_1 41 //LSB of distance
#define STEP_A_2 42
#define STEP_A_4 43 //MSB of distance

//Input pins to control motor driver "B"
#define INDIR_PIN_B 44 //direction
#define STEP_B_1 45 //LSB of step distance
#define STEP_B_2 46
#define STEP_B_4 47 //MSB of distance

//Button input to move
#define MOVE 22

// Output for electromagnet
#define MAGNET 23

//setup is run first, then loop is repeated forever

void setup() {
  pinMode(DIR_PIN_A, OUTPUT);
  pinMode(STEP_PIN_A, OUTPUT);
  
  pinMode(DIR_PIN_B, OUTPUT);
  pinMode(STEP_PIN_B, OUTPUT);
  
  pinMode(MAGNET, OUTPUT);
  
  pinMode(INDIR_PIN_A, INPUT);
  pinMode(STEP_A_1, INPUT);
  pinMode(STEP_A_2, INPUT);
  pinMode(STEP_A_4, INPUT);
  
  pinMode(INDIR_PIN_B, INPUT);
  pinMode(STEP_B_1, INPUT);
  pinMode(STEP_B_2, INPUT);
  pinMode(STEP_B_4, INPUT);
  
  pinMode(MOVE, INPUT);
}

//If an input is 3+ volts it's HIGH, if it's 2- volts it's LOW
//for an output, 5 volts is HIGH, 0 volts is LOW
void loop() {
  int spaces_a = (digitalRead(STEP_A_1) == HIGH) ? 1 : 0;
  int spaces_b = (digitalRead(STEP_B_1) == HIGH) ? 1 : 0;
  int dir_a = (digitalRead(INDIR_PIN_A) == HIGH) ? 1 : -1;
  int dir_b = (digitalRead(INDIR_PIN_B) == HIGH) ? 1 : -1;
  int distance_a = 0;
  int distance_b = 0;
  float ramp = .1;
  
  spaces_a = spaces_a + ((digitalRead(STEP_A_2) == HIGH) ? 2 : 0 );
  spaces_a = spaces_a + ((digitalRead(STEP_A_4) == HIGH) ? 4 : 0 );
  
  spaces_b = spaces_b + ((digitalRead(STEP_B_2) == HIGH) ? 2 : 0 );
  spaces_b = spaces_b + ((digitalRead(STEP_B_4) == HIGH) ? 4 : 0 );

  distance_a = spaces_a * 1900 * 3;
  distance_b = spaces_b * 3750 * 3;
  
  if(digitalRead(MOVE) == HIGH) {
    digitalWrite(MAGNET, HIGH);
    ramp = .1;
    //move a certain number of centimeters
    while(distance_a != 0) {
      rotatea((950 * dir_a), ramp);
      distance_a = distance_a - 950;
      ramp += 0.3;
    }
    ramp = .1;
    while(distance_b != 0) {
      rotateb((1875 * dir_b), ramp);
      distance_b = distance_b - 1875;
      ramp += 0.3;
    }
    digitalWrite(MAGNET, LOW);
    delay(1000);
  }
}

void rotatea(int steps, float speed) {
  //rotate a specific number of microsteps (8 microsteps per step) - (negative for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger
  int dir = (steps > 0)? HIGH:LOW;
  steps = abs(steps);
  
  digitalWrite(DIR_PIN_A, dir);

  if(speed > 1) {
    speed = 1;
  }

  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){
    digitalWrite(STEP_PIN_A, HIGH);
    delayMicroseconds(usDelay); 

    digitalWrite(STEP_PIN_A, LOW);
    delayMicroseconds(usDelay);
  }
}

void rotateb(int steps, float speed) {
  //rotate a specific number of microsteps (8 microsteps per step) - (negative for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger
  int dir = (steps > 0)? HIGH:LOW;
  steps = abs(steps);

  digitalWrite(DIR_PIN_B, dir);

  if(speed > 1) {
    speed = 1;
  }

  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){
    digitalWrite(STEP_PIN_B, HIGH);
    delayMicroseconds(usDelay); 

    digitalWrite(STEP_PIN_B, LOW);
    delayMicroseconds(usDelay);
  }
}
